#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "ch.h"
#include "hal.h"
#include "chprintf.h"
#include "nanosdr.h"
#include "arm_math.h"

// result is 8.8 format
uint16_t
log2_q31(uint32_t x)
{
	uint32_t mask = 0xffff0000;
	uint16_t bit = 16;
	uint16_t y = 32;//-15;
	uint8_t i;

	if (x == 0)
		return 0;
	// 16
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 8
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 4
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 2
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 1
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	// msb should be 1. take next 8 bits.
	i = (x >> 23) & 0xff;
	// lookup logarythm table
	return (y << 8) | i;
}

static inline uint16_t
log2_i64(uint64_t x)
{
	uint64_t mask = 0xffffffff00000000;
	uint16_t bit = 32;
	int16_t y = 63;
	uint8_t i;

	if (x == 0)
		return 0;
	// 32
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 16
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 8
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 4
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 2
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	bit >>= 1;
	mask <<= bit;
	// 1
	if ((x & mask) == 0) {
		x <<= bit;
		y -= bit;
	}
	// msb should be 1. take next 8 bits.
	i = (x >> (63-8)) & 0xff;
	// lookup logarythm table
	return (y << 8) | i;
}


// half of 1024pt window function
const int16_t winfunc_hamming[] = {
         // hamming
		 2621,  2622,  2622,  2624,  2626,  2628,  2632,  2635,
		 2640,  2644,  2650,  2656,  2662,  2669,  2677,  2685,
		 2694,  2703,  2713,  2724,  2735,  2747,  2759,  2772,
		 2785,  2799,  2813,  2828,  2844,  2860,  2877,  2894,
		 2912,  2930,  2949,  2968,  2988,  3009,  3030,  3052,
		 3074,  3097,  3120,  3144,  3168,  3193,  3219,  3245,
		 3272,  3299,  3327,  3355,  3384,  3413,  3443,  3473,
		 3504,  3536,  3568,  3600,  3633,  3667,  3701,  3736,
		 3771,  3807,  3843,  3880,  3917,  3955,  3993,  4032,
		 4071,  4111,  4152,  4192,  4234,  4276,  4318,  4361,
		 4405,  4448,  4493,  4538,  4583,  4629,  4676,  4722,
		 4770,  4818,  4866,  4915,  4964,  5014,  5064,  5115,
		 5166,  5218,  5270,  5323,  5376,  5430,  5484,  5538,
		 5593,  5649,  5704,  5761,  5818,  5875,  5932,  5991,
		 6049,  6108,  6168,  6227,  6288,  6348,  6410,  6471,
		 6533,  6596,  6659,  6722,  6785,  6850,  6914,  6979,
		 7044,  7110,  7176,  7243,  7310,  7377,  7444,  7513,
		 7581,  7650,  7719,  7789,  7859,  7929,  8000,  8071,
		 8142,  8214,  8286,  8359,  8431,  8505,  8578,  8652,
		 8726,  8801,  8876,  8951,  9027,  9103,  9179,  9255,
		 9332,  9409,  9487,  9565,  9643,  9721,  9800,  9879,
		 9958, 10038, 10118, 10198, 10278, 10359, 10440, 10521,
		10603, 10685, 10767, 10849, 10932, 11015, 11098, 11181,
		11265, 11349, 11433, 11517, 11602, 11686, 11771, 11857,
		11942, 12028, 12114, 12200, 12286, 12373, 12459, 12546,
		12633, 12721, 12808, 12896, 12984, 13072, 13160, 13248,
		13337, 13425, 13514, 13603, 13693, 13782, 13871, 13961,
		14051, 14141, 14231, 14321, 14411, 14502, 14592, 14683,
		14773, 14864, 14955, 15046, 15138, 15229, 15320, 15412,
		15503, 15595, 15687, 15778, 15870, 15962, 16054, 16146,
		16238, 16331, 16423, 16515, 16607, 16700, 16792, 16885,
		16977, 17069, 17162, 17255, 17347, 17440, 17532, 17625,
		17717, 17810, 17902, 17995, 18088, 18180, 18273, 18365,
		18458, 18550, 18642, 18735, 18827, 18919, 19012, 19104,
		19196, 19288, 19380, 19472, 19564, 19656, 19748, 19839,
		19931, 20022, 20114, 20205, 20296, 20387, 20479, 20569,
		20660, 20751, 20842, 20932, 21022, 21113, 21203, 21293,
		21383, 21472, 21562, 21651, 21740, 21830, 21918, 22007,
		22096, 22184, 22273, 22361, 22449, 22536, 22624, 22711,
		22799, 22886, 22972, 23059, 23145, 23232, 23318, 23403,
		23489, 23574, 23659, 23744, 23829, 23914, 23998, 24082,
		24165, 24249, 24332, 24415, 24498, 24580, 24663, 24745,
		24826, 24908, 24989, 25070, 25150, 25231, 25311, 25390,
		25470, 25549, 25628, 25706, 25785, 25863, 25940, 26018,
		26095, 26171, 26248, 26324, 26400, 26475, 26550, 26625,
		26699, 26773, 26847, 26920, 26993, 27066, 27138, 27210,
		27282, 27353, 27424, 27495, 27565, 27635, 27704, 27773,
		27842, 27910, 27978, 28045, 28112, 28179, 28245, 28311,
		28377, 28442, 28507, 28571, 28635, 28698, 28761, 28824,
		28886, 28948, 29009, 29070, 29131, 29191, 29251, 29310,
		29369, 29427, 29485, 29542, 29599, 29656, 29712, 29768,
		29823, 29877, 29932, 29986, 30039, 30092, 30144, 30196,
		30248, 30299, 30349, 30399, 30449, 30498, 30546, 30595,
		30642, 30689, 30736, 30782, 30828, 30873, 30918, 30962,
		31006, 31049, 31091, 31134, 31175, 31216, 31257, 31297,
		31337, 31376, 31414, 31453, 31490, 31527, 31564, 31600,
		31635, 31670, 31704, 31738, 31772, 31804, 31837, 31869,
		31900, 31930, 31961, 31990, 32019, 32048, 32076, 32103,
		32130, 32156, 32182, 32208, 32232, 32256, 32280, 32303,
		32326, 32348, 32369, 32390, 32410, 32430, 32449, 32468,
		32486, 32503, 32520, 32537, 32553, 32568, 32583, 32597,
		32610, 32623, 32636, 32648, 32659, 32670, 32680, 32690,
		32699, 32707, 32715, 32723, 32729, 32736, 32741, 32746,
		32751, 32755, 32758, 32761, 32764, 32765, 32766, 32767,

		32767, 32766, 32765, 32764, 32761, 32758, 32755, 32751,
		32746, 32741, 32736, 32729, 32723, 32715, 32707, 32699,
		32690, 32680, 32670, 32659, 32648, 32636, 32623, 32610,
		32597, 32583, 32568, 32553, 32537, 32520, 32503, 32486,
		32468, 32449, 32430, 32410, 32390, 32369, 32348, 32326,
		32303, 32280, 32256, 32232, 32208, 32182, 32156, 32130,
		32103, 32076, 32048, 32019, 31990, 31961, 31930, 31900,
		31869, 31837, 31804, 31772, 31738, 31704, 31670, 31635,
		31600, 31564, 31527, 31490, 31453, 31414, 31376, 31337,
		31297, 31257, 31216, 31175, 31134, 31091, 31049, 31006,
		30962, 30918, 30873, 30828, 30782, 30736, 30689, 30642,
		30595, 30546, 30498, 30449, 30399, 30349, 30299, 30248,
		30196, 30144, 30092, 30039, 29986, 29932, 29877, 29823,
		29768, 29712, 29656, 29599, 29542, 29485, 29427, 29369,
		29310, 29251, 29191, 29131, 29070, 29009, 28948, 28886,
		28824, 28761, 28698, 28635, 28571, 28507, 28442, 28377,
		28311, 28245, 28179, 28112, 28045, 27978, 27910, 27842,
		27773, 27704, 27635, 27565, 27495, 27424, 27353, 27282,
		27210, 27138, 27066, 26993, 26920, 26847, 26773, 26699,
		26625, 26550, 26475, 26400, 26324, 26248, 26171, 26095,
		26018, 25940, 25863, 25785, 25706, 25628, 25549, 25470,
		25390, 25311, 25231, 25150, 25070, 24989, 24908, 24826,
		24745, 24663, 24580, 24498, 24415, 24332, 24249, 24165,
		24082, 23998, 23914, 23829, 23744, 23659, 23574, 23489,
		23403, 23318, 23232, 23145, 23059, 22972, 22886, 22799,
		22711, 22624, 22536, 22449, 22361, 22273, 22184, 22096,
		22007, 21918, 21830, 21740, 21651, 21562, 21472, 21383,
		21293, 21203, 21113, 21022, 20932, 20842, 20751, 20660,
		20569, 20479, 20387, 20296, 20205, 20114, 20022, 19931,
		19839, 19748, 19656, 19564, 19472, 19380, 19288, 19196,
		19104, 19012, 18919, 18827, 18735, 18642, 18550, 18458,
		18365, 18273, 18180, 18088, 17995, 17902, 17810, 17717,
		17625, 17532, 17440, 17347, 17255, 17162, 17069, 16977,
		16885, 16792, 16700, 16607, 16515, 16423, 16331, 16238,
		16146, 16054, 15962, 15870, 15778, 15687, 15595, 15503,
		15412, 15320, 15229, 15138, 15046, 14955, 14864, 14773,
		14683, 14592, 14502, 14411, 14321, 14231, 14141, 14051,
		13961, 13871, 13782, 13693, 13603, 13514, 13425, 13337,
		13248, 13160, 13072, 12984, 12896, 12808, 12721, 12633,
		12546, 12459, 12373, 12286, 12200, 12114, 12028, 11942,
		11857, 11771, 11686, 11602, 11517, 11433, 11349, 11265,
		11181, 11098, 11015, 10932, 10849, 10767, 10685, 10603,
		10521, 10440, 10359, 10278, 10198, 10118, 10038,  9958,
		 9879,  9800,  9721,  9643,  9565,  9487,  9409,  9332,
		 9255,  9179,  9103,  9027,  8951,  8876,  8801,  8726,
		 8652,  8578,  8505,  8431,  8359,  8286,  8214,  8142,
		 8071,  8000,  7929,  7859,  7789,  7719,  7650,  7581,
		 7513,  7444,  7377,  7310,  7243,  7176,  7110,  7044,
		 6979,  6914,  6850,  6785,  6722,  6659,  6596,  6533,
		 6471,  6410,  6348,  6288,  6227,  6168,  6108,  6049,
		 5991,  5932,  5875,  5818,  5761,  5704,  5649,  5593,
		 5538,  5484,  5430,  5376,  5323,  5270,  5218,  5166,
		 5115,  5064,  5014,  4964,  4915,  4866,  4818,  4770,
		 4722,  4676,  4629,  4583,  4538,  4493,  4448,  4405,
		 4361,  4318,  4276,  4234,  4192,  4152,  4111,  4071,
		 4032,  3993,  3955,  3917,  3880,  3843,  3807,  3771,
		 3736,  3701,  3667,  3633,  3600,  3568,  3536,  3504,
		 3473,  3443,  3413,  3384,  3355,  3327,  3299,  3272,
		 3245,  3219,  3193,  3168,  3144,  3120,  3097,  3074,
		 3052,  3030,  3009,  2988,  2968,  2949,  2930,  2912,
		 2894,  2877,  2860,  2844,  2828,  2813,  2799,  2785,
		 2772,  2759,  2747,  2735,  2724,  2713,  2703,  2694,
		 2685,  2677,  2669,  2662,  2656,  2650,  2644,  2640,
		 2635,  2632,  2628,  2626,  2624,  2622,  2622,  2621
};

const int16_t winfunc_blackmanharris[] = {
		 // blackman-harris
		    2,     2,     2,     2,     2,     2,     3,     3,
		    3,     3,     4,     4,     5,     5,     5,     6,
		    7,     7,     8,     8,     9,    10,    11,    12,
		   13,    14,    15,    16,    17,    18,    19,    20,
		   22,    23,    24,    26,    27,    29,    31,    32,
		   34,    36,    38,    40,    42,    44,    46,    48,
		   51,    53,    56,    58,    61,    64,    67,    70,
		   73,    76,    79,    82,    86,    89,    93,    97,
		  100,   104,   109,   113,   117,   122,   126,   131,
		  136,   141,   146,   151,   156,   162,   168,   173,
		  179,   186,   192,   198,   205,   212,   219,   226,
		  233,   241,   249,   256,   265,   273,   281,   290,
		  299,   308,   317,   327,   337,   347,   357,   367,
		  378,   389,   400,   411,   423,   435,   447,   459,
		  472,   485,   498,   512,   526,   540,   554,   569,
		  584,   599,   614,   630,   646,   663,   680,   697,
		  714,   732,   750,   769,   788,   807,   827,   846,
		  867,   887,   908,   930,   952,   974,   996,  1019,
		 1043,  1066,  1091,  1115,  1140,  1166,  1192,  1218,
		 1245,  1272,  1299,  1327,  1356,  1385,  1414,  1444,
		 1475,  1506,  1537,  1569,  1601,  1634,  1667,  1701,
		 1735,  1770,  1805,  1841,  1878,  1915,  1952,  1990,
		 2029,  2068,  2107,  2147,  2188,  2229,  2271,  2314,
		 2357,  2400,  2444,  2489,  2534,  2580,  2627,  2674,
		 2721,  2770,  2819,  2868,  2918,  2969,  3020,  3072,
		 3125,  3178,  3232,  3287,  3342,  3398,  3454,  3511,
		 3569,  3628,  3687,  3746,  3807,  3868,  3930,  3992,
		 4055,  4119,  4183,  4249,  4314,  4381,  4448,  4516,
		 4585,  4654,  4724,  4794,  4866,  4938,  5010,  5084,
		 5158,  5233,  5308,  5385,  5462,  5539,  5618,  5697,
		 5776,  5857,  5938,  6020,  6103,  6186,  6270,  6355,
		 6440,  6526,  6613,  6700,  6789,  6878,  6967,  7058,
		 7149,  7240,  7333,  7426,  7520,  7614,  7710,  7805,
		 7902,  7999,  8097,  8196,  8295,  8395,  8496,  8597,
		 8699,  8802,  8905,  9009,  9114,  9219,  9325,  9431,
		 9539,  9646,  9755,  9864,  9974, 10084, 10195, 10306,
		10419, 10531, 10645, 10759, 10873, 10988, 11104, 11220,
		11337, 11454, 11572, 11691, 11810, 11929, 12049, 12170,
		12291, 12412, 12534, 12657, 12780, 12903, 13027, 13152,
		13277, 13402, 13528, 13654, 13781, 13908, 14035, 14163,
		14291, 14420, 14549, 14678, 14808, 14938, 15068, 15199,
		15330, 15461, 15593, 15725, 15857, 15989, 16122, 16255,
		16388, 16522, 16655, 16789, 16923, 17058, 17192, 17327,
		17461, 17596, 17731, 17867, 18002, 18137, 18273, 18408,
		18544, 18680, 18815, 18951, 19087, 19223, 19358, 19494,
		19630, 19765, 19901, 20037, 20172, 20308, 20443, 20578,
		20713, 20848, 20983, 21118, 21252, 21386, 21520, 21654,
		21788, 21921, 22054, 22187, 22320, 22452, 22584, 22716,
		22847, 22978, 23109, 23239, 23369, 23499, 23628, 23757,
		23885, 24013, 24140, 24267, 24394, 24520, 24645, 24770,
		24894, 25018, 25142, 25264, 25386, 25508, 25629, 25749,
		25869, 25988, 26106, 26224, 26341, 26457, 26573, 26688,
		26802, 26915, 27028, 27139, 27251, 27361, 27470, 27579,
		27687, 27794, 27900, 28005, 28109, 28213, 28315, 28417,
		28518, 28617, 28716, 28814, 28911, 29007, 29102, 29196,
		29288, 29380, 29471, 29561, 29650, 29737, 29824, 29909,
		29994, 30077, 30159, 30240, 30320, 30399, 30477, 30553,
		30628, 30702, 30775, 30847, 30918, 30987, 31055, 31122,
		31188, 31252, 31315, 31377, 31438, 31497, 31555, 31612,
		31667, 31721, 31774, 31826, 31876, 31925, 31972, 32019,
		32064, 32107, 32149, 32190, 32230, 32268, 32304, 32340,
		32374, 32406, 32438, 32467, 32496, 32523, 32548, 32573,
		32595, 32617, 32637, 32655, 32672, 32688, 32702, 32715,
		32727, 32737, 32745, 32753, 32758, 32763, 32765, 32767,

		32767, 32765, 32763, 32758, 32753, 32745, 32737, 32727,
		32715, 32702, 32688, 32672, 32655, 32637, 32617, 32595,
		32573, 32548, 32523, 32496, 32467, 32438, 32406, 32374,
		32340, 32304, 32268, 32230, 32190, 32149, 32107, 32064,
		32019, 31972, 31925, 31876, 31826, 31774, 31721, 31667,
		31612, 31555, 31497, 31438, 31377, 31315, 31252, 31188,
		31122, 31055, 30987, 30918, 30847, 30775, 30702, 30628,
		30553, 30477, 30399, 30320, 30240, 30159, 30077, 29994,
		29909, 29824, 29737, 29650, 29561, 29471, 29380, 29288,
		29196, 29102, 29007, 28911, 28814, 28716, 28617, 28518,
		28417, 28315, 28213, 28109, 28005, 27900, 27794, 27687,
		27579, 27470, 27361, 27251, 27139, 27028, 26915, 26802,
		26688, 26573, 26457, 26341, 26224, 26106, 25988, 25869,
		25749, 25629, 25508, 25386, 25264, 25142, 25018, 24894,
		24770, 24645, 24520, 24394, 24267, 24140, 24013, 23885,
		23757, 23628, 23499, 23369, 23239, 23109, 22978, 22847,
		22716, 22584, 22452, 22320, 22187, 22054, 21921, 21788,
		21654, 21520, 21386, 21252, 21118, 20983, 20848, 20713,
		20578, 20443, 20308, 20172, 20037, 19901, 19765, 19630,
		19494, 19358, 19223, 19087, 18951, 18815, 18680, 18544,
		18408, 18273, 18137, 18002, 17867, 17731, 17596, 17461,
		17327, 17192, 17058, 16923, 16789, 16655, 16522, 16388,
		16255, 16122, 15989, 15857, 15725, 15593, 15461, 15330,
		15199, 15068, 14938, 14808, 14678, 14549, 14420, 14291,
		14163, 14035, 13908, 13781, 13654, 13528, 13402, 13277,
		13152, 13027, 12903, 12780, 12657, 12534, 12412, 12291,
		12170, 12049, 11929, 11810, 11691, 11572, 11454, 11337,
		11220, 11104, 10988, 10873, 10759, 10645, 10531, 10419,
		10306, 10195, 10084,  9974,  9864,  9755,  9646,  9539,
		 9431,  9325,  9219,  9114,  9009,  8905,  8802,  8699,
		 8597,  8496,  8395,  8295,  8196,  8097,  7999,  7902,
		 7805,  7710,  7614,  7520,  7426,  7333,  7240,  7149,
		 7058,  6967,  6878,  6789,  6700,  6613,  6526,  6440,
		 6355,  6270,  6186,  6103,  6020,  5938,  5857,  5776,
		 5697,  5618,  5539,  5462,  5385,  5308,  5233,  5158,
		 5084,  5010,  4938,  4866,  4794,  4724,  4654,  4585,
		 4516,  4448,  4381,  4314,  4249,  4183,  4119,  4055,
		 3992,  3930,  3868,  3807,  3746,  3687,  3628,  3569,
		 3511,  3454,  3398,  3342,  3287,  3232,  3178,  3125,
		 3072,  3020,  2969,  2918,  2868,  2819,  2770,  2721,
		 2674,  2627,  2580,  2534,  2489,  2444,  2400,  2357,
		 2314,  2271,  2229,  2188,  2147,  2107,  2068,  2029,
		 1990,  1952,  1915,  1878,  1841,  1805,  1770,  1735,
		 1701,  1667,  1634,  1601,  1569,  1537,  1506,  1475,
		 1444,  1414,  1385,  1356,  1327,  1299,  1272,  1245,
		 1218,  1192,  1166,  1140,  1115,  1091,  1066,  1043,
		 1019,   996,   974,   952,   930,   908,   887,   867,
		  846,   827,   807,   788,   769,   750,   732,   714,
		  697,   680,   663,   646,   630,   614,   599,   584,
		  569,   554,   540,   526,   512,   498,   485,   472,
		  459,   447,   435,   423,   411,   400,   389,   378,
		  367,   357,   347,   337,   327,   317,   308,   299,
		  290,   281,   273,   265,   256,   249,   241,   233,
		  226,   219,   212,   205,   198,   192,   186,   179,
		  173,   168,   162,   156,   151,   146,   141,   136,
		  131,   126,   122,   117,   113,   109,   104,   100,
		   97,    93,    89,    86,    82,    79,    76,    73,
		   70,    67,    64,    61,    58,    56,    53,    51,
		   48,    46,    44,    42,    40,    38,    36,    34,
		   32,    31,    29,    27,    26,    24,    23,    22,
		   20,    19,    18,    17,    16,    15,    14,    13,
		   12,    11,    10,     9,     8,     8,     7,     7,
		    6,     5,     5,     5,     4,     4,     3,     3,
		    3,     3,     2,     2,     2,     2,     2,     2
};

const int16_t winfunc_chebychef[] = {
         // chebychef
   67,    10,    10,    11,    12,    13,    14,    14,
   15,    16,    17,    18,    19,    21,    22,    23,
   24,    26,    27,    28,    30,    31,    33,    35,
   36,    38,    40,    42,    44,    46,    48,    50,
   52,    54,    57,    59,    62,    64,    67,    70,
   73,    76,    79,    82,    85,    88,    92,    95,
   99,   103,   107,   111,   115,   119,   123,   128,
  132,   137,   141,   146,   151,   157,   162,   167,
  173,   179,   185,   191,   197,   203,   210,   216,
  223,   230,   237,   245,   252,   260,   268,   276,
  284,   292,   301,   310,   319,   328,   337,   347,
  357,   367,   377,   387,   398,   409,   420,   432,
  443,   455,   467,   480,   492,   505,   518,   532,
  545,   559,   573,   588,   603,   618,   633,   649,
  665,   681,   697,   714,   731,   749,   767,   785,
  803,   822,   841,   861,   880,   900,   921,   942,
  963,   985,  1006,  1029,  1051,  1074,  1098,  1122,
 1146,  1170,  1195,  1221,  1247,  1273,  1299,  1327,
 1354,  1382,  1410,  1439,  1468,  1498,  1528,  1558,
 1589,  1621,  1653,  1685,  1718,  1751,  1785,  1819,
 1854,  1889,  1925,  1961,  1998,  2035,  2073,  2111,
 2150,  2189,  2229,  2269,  2310,  2352,  2394,  2436,
 2479,  2523,  2567,  2611,  2657,  2702,  2749,  2796,
 2843,  2891,  2940,  2989,  3039,  3089,  3140,  3192,
 3244,  3297,  3350,  3404,  3459,  3514,  3570,  3626,
 3683,  3741,  3799,  3858,  3917,  3977,  4038,  4100,
 4162,  4224,  4288,  4351,  4416,  4481,  4547,  4614,
 4681,  4749,  4817,  4886,  4956,  5026,  5097,  5169,
 5242,  5315,  5388,  5463,  5538,  5613,  5690,  5767,
 5844,  5923,  6002,  6081,  6162,  6243,  6324,  6407,
 6490,  6573,  6658,  6743,  6828,  6914,  7001,  7089,
 7177,  7266,  7356,  7446,  7537,  7628,  7720,  7813,
 7907,  8001,  8095,  8191,  8287,  8383,  8481,  8578,
 8677,  8776,  8876,  8976,  9077,  9179,  9281,  9384,
 9487,  9591,  9696,  9801,  9907, 10013, 10120, 10228,
10336, 10444, 10554, 10663, 10774, 10884, 10996, 11108,
11220, 11333, 11447, 11561, 11675, 11790, 11906, 12022,
12139, 12256, 12373, 12491, 12610, 12728, 12848, 12968,
13088, 13208, 13329, 13451, 13573, 13695, 13818, 13941,
14064, 14188, 14312, 14437, 14562, 14687, 14813, 14939,
15065, 15191, 15318, 15445, 15573, 15700, 15828, 15956,
16085, 16214, 16342, 16472, 16601, 16730, 16860, 16990,
17120, 17250, 17381, 17511, 17642, 17773, 17904, 18035,
18166, 18297, 18428, 18560, 18691, 18823, 18954, 19086,
19217, 19349, 19480, 19611, 19743, 19874, 20006, 20137,
20268, 20399, 20530, 20661, 20792, 20922, 21053, 21183,
21313, 21443, 21573, 21703, 21832, 21961, 22090, 22219,
22347, 22475, 22603, 22731, 22858, 22985, 23111, 23238,
23363, 23489, 23614, 23739, 23863, 23987, 24111, 24234,
24356, 24479, 24600, 24721, 24842, 24962, 25082, 25201,
25320, 25438, 25555, 25672, 25789, 25904, 26020, 26134,
26248, 26361, 26474, 26586, 26697, 26807, 26917, 27026,
27135, 27242, 27349, 27455, 27561, 27665, 27769, 27872,
27974, 28076, 28176, 28276, 28375, 28473, 28570, 28666,
28761, 28856, 28949, 29042, 29133, 29224, 29314, 29402,
29490, 29577, 29663, 29748, 29831, 29914, 29996, 30077,
30156, 30235, 30312, 30389, 30464, 30539, 30612, 30684,
30755, 30825, 30893, 30961, 31028, 31093, 31157, 31220,
31282, 31342, 31402, 31460, 31517, 31573, 31628, 31681,
31733, 31784, 31834, 31882, 31930, 31976, 32020, 32064,
32106, 32147, 32187, 32225, 32262, 32298, 32333, 32366,
32398, 32428, 32458, 32486, 32512, 32538, 32562, 32585,
32606, 32626, 32645, 32662, 32678, 32693, 32707, 32719,
32729, 32739, 32747, 32754, 32759, 32763, 32766, 32767,
32767, 32766, 32763, 32759, 32754, 32747, 32739, 32729,
32719, 32707, 32693, 32678, 32662, 32645, 32626, 32606,
32585, 32562, 32538, 32512, 32486, 32458, 32428, 32398,
32366, 32333, 32298, 32262, 32225, 32187, 32147, 32106,
32064, 32020, 31976, 31930, 31882, 31834, 31784, 31733,
31681, 31628, 31573, 31517, 31460, 31402, 31342, 31282,
31220, 31157, 31093, 31028, 30961, 30893, 30825, 30755,
30684, 30612, 30539, 30464, 30389, 30312, 30235, 30156,
30077, 29996, 29914, 29831, 29748, 29663, 29577, 29490,
29402, 29314, 29224, 29133, 29042, 28949, 28856, 28761,
28666, 28570, 28473, 28375, 28276, 28176, 28076, 27974,
27872, 27769, 27665, 27561, 27455, 27349, 27242, 27135,
27026, 26917, 26807, 26697, 26586, 26474, 26361, 26248,
26134, 26020, 25904, 25789, 25672, 25555, 25438, 25320,
25201, 25082, 24962, 24842, 24721, 24600, 24479, 24356,
24234, 24111, 23987, 23863, 23739, 23614, 23489, 23363,
23238, 23111, 22985, 22858, 22731, 22603, 22475, 22347,
22219, 22090, 21961, 21832, 21703, 21573, 21443, 21313,
21183, 21053, 20922, 20792, 20661, 20530, 20399, 20268,
20137, 20006, 19874, 19743, 19611, 19480, 19349, 19217,
19086, 18954, 18823, 18691, 18560, 18428, 18297, 18166,
18035, 17904, 17773, 17642, 17511, 17381, 17250, 17120,
16990, 16860, 16730, 16601, 16472, 16342, 16214, 16085,
15956, 15828, 15700, 15573, 15445, 15318, 15191, 15065,
14939, 14813, 14687, 14562, 14437, 14312, 14188, 14064,
13941, 13818, 13695, 13573, 13451, 13329, 13208, 13088,
12968, 12848, 12728, 12610, 12491, 12373, 12256, 12139,
12022, 11906, 11790, 11675, 11561, 11447, 11333, 11220,
11108, 10996, 10884, 10774, 10663, 10554, 10444, 10336,
10228, 10120, 10013,  9907,  9801,  9696,  9591,  9487,
 9384,  9281,  9179,  9077,  8976,  8876,  8776,  8677,
 8578,  8481,  8383,  8287,  8191,  8095,  8001,  7907,
 7813,  7720,  7628,  7537,  7446,  7356,  7266,  7177,
 7089,  7001,  6914,  6828,  6743,  6658,  6573,  6490,
 6407,  6324,  6243,  6162,  6081,  6002,  5923,  5844,
 5767,  5690,  5613,  5538,  5463,  5388,  5315,  5242,
 5169,  5097,  5026,  4956,  4886,  4817,  4749,  4681,
 4614,  4547,  4481,  4416,  4351,  4288,  4224,  4162,
 4100,  4038,  3977,  3917,  3858,  3799,  3741,  3683,
 3626,  3570,  3514,  3459,  3404,  3350,  3297,  3244,
 3192,  3140,  3089,  3039,  2989,  2940,  2891,  2843,
 2796,  2749,  2702,  2657,  2611,  2567,  2523,  2479,
 2436,  2394,  2352,  2310,  2269,  2229,  2189,  2150,
 2111,  2073,  2035,  1998,  1961,  1925,  1889,  1854,
 1819,  1785,  1751,  1718,  1685,  1653,  1621,  1589,
 1558,  1528,  1498,  1468,  1439,  1410,  1382,  1354,
 1327,  1299,  1273,  1247,  1221,  1195,  1170,  1146,
 1122,  1098,  1074,  1051,  1029,  1006,   985,   963,
  942,   921,   900,   880,   861,   841,   822,   803,
  785,   767,   749,   731,   714,   697,   681,   665,
  649,   633,   618,   603,   588,   573,   559,   545,
  532,   518,   505,   492,   480,   467,   455,   443,
  432,   420,   409,   398,   387,   377,   367,   357,
  347,   337,   328,   319,   310,   301,   292,   284,
  276,   268,   260,   252,   245,   237,   230,   223,
  216,   210,   203,   197,   191,   185,   179,   173,
  167,   162,   157,   151,   146,   141,   137,   132,
  128,   123,   119,   115,   111,   107,   103,    99,
   95,    92,    88,    85,    82,    79,    76,    73,
   70,    67,    64,    62,    59,    57,    54,    52,
   50,    48,    46,    44,    42,    40,    38,    36,
   35,    33,    31,    30,    28,    27,    26,    24,
   23,    22,    21,    19,    18,    17,    16,    15,
   14,    14,    13,    12,    11,    10,    10,    67
};

const int16_t *winfunc_table = winfunc_hamming;

void
set_window_function(int wf_type)
{
  if (wf_type == 0)
    winfunc_table = winfunc_blackmanharris;
  else if (wf_type == 1)
    winfunc_table = winfunc_hamming;
  else if (wf_type == 2)
    winfunc_table = winfunc_chebychef;
}
            
arm_cfft_radix4_instance_q31 cfft_inst;

//#define mag(r,i) (q31_t)(((q63_t)r*r)>>33)+(q31_t)(((q63_t)i*i)>>33)

static int mag_shift = 0;


#define BG_ACTIVE RGB565(15,10,10)
#define BG_NORMAL 0x0000

#define FG_ACTIVE RGB565(128,255,128)
#define FG_NORMAL 0xffff


uistat_t uistat;


typedef struct {
	uint32_t sample_freq;
	int16_t offset;
	int16_t stride;
	int16_t overgain;

	int16_t origin;
	int16_t tickstep;
	int16_t tickbase;
	int16_t tickunit;
	const char *unitname;
} spectrumdisplay_param_t;

// 320pixel = 1024pt = 48kHz
// 35.55 pixel = 5kHz

const spectrumdisplay_param_t spdispparam_tbl[SPDISP_MODE_MAX] = {
  // sps, off, stride, gain,   origin, step, base, unit, unitname
  { 48000, -160*3, 3, 0,           160, 36, 0, 5, "kHz" },
  { 48000, -160*3, 3, 0,           160, 36, 0, 5, "kHz" },
  { 48000, -160*2, 2, 0,           160, 21, 0, 2, "kHz" },
  { 48000,      0, 1, 0,             0, 43, 0, 2, "kHz" }
};

const spectrumdisplay_param_t spdispparam_tbl_192khz[SPDISP_MODE_MAX] = {
  // sps, off, stride, gain,   origin, step, base, unit, unitname
  { 192000, -160*3, 3, 0,           160, 36, 0, 20, "kHz" },
  { 192000, -160*3, 3, 0,           160, 36, 0, 20, "kHz" },
  { 192000, -160*2, 2, 0,           160, 26, 0, 10, "kHz" },
  { 192000,      0, 1, 0,             0, 26, 0, 5, "kHz" }
};

#define GET_SPDISP_PARAM(mode) \
  ((uistat.fs == 192) ? &spdispparam_tbl_192khz[mode] : &spdispparam_tbl[mode])


// when event sent with SEV from M4 core, filled following data
typedef struct {
	q31_t *buffer;
	uint32_t buffer_rest;
	uint8_t update_flag;
} spectrumdisplay_t;

// update_flag
#define FLAG_SPDISP 	(1<<0)
#define FLAG_POWER 		(1<<1)
#define FLAG_UI 		(1<<2)
#define FLAG_AUX_INFO	(1<<3)


spectrumdisplay_t spdispinfo;


#define SPDISP_BUFFER_LENGTH 2048
q31_t SPDISP_BUFFER[SPDISP_BUFFER_LENGTH];

q31_t *spdisp_fetch_current = SPDISP_BUFFER;
int16_t spdisp_fetch_rest = 0;
const int16_t *spdisp_wf_current = winfunc_blackmanharris;



__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMULBB(uint32_t op1, uint32_t op2)
{
  uint32_t result;
  __ASM volatile ("smulbb %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
  return(result);
}
__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMULTT(uint32_t op1, uint32_t op2)
{
  uint32_t result;
  __ASM volatile ("smultt %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
  return(result);
}
__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMULTB(uint32_t op1, uint32_t op2)
{
  uint32_t result;
  __ASM volatile ("smultb %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
  return(result);
}
__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMULBT(uint32_t op1, uint32_t op2)
{
  uint32_t result;
  __ASM volatile ("smulbt %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
  return(result);
}


// copy samples from 16bit into 32bit with applying window function
inline static void
window_complex_15to31(q15_t *s1, q15_t *s2, size_t length)
{
    q31_t *dest = spdisp_fetch_current;
    const q15_t *wf = spdisp_wf_current;
    spdisp_fetch_current += length * 2;
	spdisp_fetch_rest -= length * 2;
    spdisp_wf_current += length;

	length /= 2;
	while (length-- > 0) {
		uint32_t w = *__SIMD32(wf)++;
		uint32_t i1i2 = *__SIMD32(s1)++;
		uint32_t q1q2 = *__SIMD32(s2)++;
		*dest++ = __SMULBB(i1i2, w) << mag_shift;
		*dest++ = __SMULBB(q1q2, w) << mag_shift;
		*dest++ = __SMULTT(i1i2, w) << mag_shift;
		*dest++ = __SMULTT(q1q2, w) << mag_shift;
	}
}

inline static void
window_real_15to31(q15_t *s1, size_t length)
{
    q31_t *dest = spdisp_fetch_current;
    const q15_t *wf = spdisp_wf_current;
    spdisp_fetch_current += length * 2;
	spdisp_fetch_rest -= length * 2;
    spdisp_wf_current += length;

	length /= 2;
	while (length-- > 0) {
		uint32_t w = *__SIMD32(wf)++;
		uint32_t i1i2 = *__SIMD32(s1)++;
		*dest++ = __SMULBB(i1i2, w) << mag_shift;
		*dest++ = 0;
		*dest++ = __SMULTT(i1i2, w) << mag_shift;
		*dest++ = 0;
	}
}

// copy samples from 16bit into 32bit with applying window function
inline static void
window_complex_interleaved_15to31(q15_t *src, size_t length)
{
    q31_t *dest = spdisp_fetch_current;
    const q15_t *wf = spdisp_wf_current;
    spdisp_fetch_current += length;
	spdisp_fetch_rest -= length;
    spdisp_wf_current += length / 2;

	length /= 4;
	while (length-- > 0) {
		uint32_t w = *__SIMD32(wf)++;
		uint32_t i1q1 = *__SIMD32(src)++;
		uint32_t i2q2 = *__SIMD32(src)++;
		*dest++ = __SMULBB(i1q1, w) << mag_shift;
		*dest++ = __SMULTB(i1q1, w) << mag_shift;
		*dest++ = __SMULBT(i2q2, w) << mag_shift;
		*dest++ = __SMULTT(i2q2, w) << mag_shift;
	}
}

inline static void
window_real_interleaved_15to31(q15_t *src, size_t length)
{
  //uint32_t offset = 0x08000800;
  //uint32_t mask = 0xFFF0FFF0;
  //uint16_t adj = spdisp_source[uistat.spdispmode].ibuf == CAPTUREBUFFER0;
    q31_t *dest = spdisp_fetch_current;
    const q15_t *wf = spdisp_wf_current;
    spdisp_fetch_current += length;
	spdisp_fetch_rest -= length;
    spdisp_wf_current += length / 2;

    length /= 4;
	while (length-- > 0) {
		uint32_t w = *__SIMD32(wf)++;
		uint32_t i1q1 = *__SIMD32(src)++;
		uint32_t i2q2 = *__SIMD32(src)++;
		//if (adj)
        //i1i2 = (__QSUB16(i1i2, offset) << 4) & mask;
		*dest++ = __SMULBB(i1q1, w) << mag_shift;
		*dest++ = 0;
		*dest++ = __SMULBT(i2q2, w) << mag_shift;
		*dest++ = 0;
	}
}

// called from dsp.c
void
disp_fetch_samples(int mode, int type, int16_t *buf0, int16_t *buf1, size_t buflen)
{
    if (mode != uistat.spdispmode)
        return;

	if (spdisp_fetch_rest == 0) {
		if (spdispinfo.update_flag & FLAG_SPDISP) {
			// currently proccessing in M0APP
			return;
		}
        // start to fetch data
		spdisp_fetch_current = SPDISP_BUFFER;
		spdisp_fetch_rest = SPDISP_BUFFER_LENGTH;
		spdisp_wf_current = winfunc_table;
	}

	size_t length = spdisp_fetch_rest;
    if (type == BT_C_INTERLEAVE || type == BT_R_INTERLEAVE) {
      if (buflen > length)
        buflen = length;
      length = buflen;
    } else {
      if (buflen > length / 2)
        buflen = length / 2;
      length = buflen * 2;
    }

	switch (type) {
    case BT_C_INTERLEAVE:
      window_complex_interleaved_15to31(buf0, buflen);
      break;
    case BT_IQ:
      window_complex_15to31(buf0, buf1, buflen);
      break;
    case BT_REAL:
      window_real_15to31(buf0, buflen);
      break;
    case BT_R_INTERLEAVE:
      window_complex_interleaved_15to31(buf0, buflen);
      break;
	}

	if (spdisp_fetch_rest == 0) {
        // filled up buffer, then analyze and draw waveform
		spdispinfo.buffer = SPDISP_BUFFER;
		spdispinfo.update_flag |= FLAG_SPDISP;
	}
}

// r:2048 c:1024 samples (8192 byte with q31_t)
//#define SPDISP_BUFFER_SIZE	8192
//static q31_t SPDISP_BUFFER[2048];

void
draw_spectrogram(void)
{
	q31_t *buf = spdispinfo.buffer;
	arm_cfft_radix4_q31(&cfft_inst, buf);

	//arm_cmplx_mag_q31(buf, buf, 1024);
//	arm_cmplx_mag_squared_q31(buf, buf, 1024);
	//draw_samples();
	//return;
	//uint16_t gainshift = spdispinfo.p.overgain;
	uint16_t mode = uistat.spdispmode;
    const spectrumdisplay_param_t *param = GET_SPDISP_PARAM(mode);
	int i = param->offset;
	int16_t stride = param->stride;
    int16_t tune_pos = param->origin;
    if (uistat.spdispmode == 0)
      tune_pos += (int)mode_freq_offset*1024 / (48000 * stride);
	uint16_t (*block)[32] = (uint16_t (*)[32])spi_buffer;
	int sx, x, y;
	for (sx = 0; sx < 320; sx += 32) {
		for (x = 0; x < 32; x++) {
          uint16_t bg = 0;
          if (sx + x == tune_pos)
            bg = RGB565(128, 255, 128);
          int i0 = i;
          int64_t acc = 0;
          for (; i < i0 + stride; i++) {
			q31_t ii = buf[(i&1023)*2];
			q31_t qq = buf[(i&1023)*2+1];
            acc += (int64_t)ii*ii + (int64_t)qq*qq;
          }
            //q31_t mag = acc>>(33-gainshift);
			//q31_t mag = buf[i & 1023];
			//int v = log2_q31(mag) >> 6;
            // format of result of log function is 8.8 format
            //int v = (log2_i64(acc) - (36<<8)) >> 6;
          int v = (log2_i64(acc) - (36<<8)) / 77; // 1dB/pixel
			if (v > 64) v = 64;
			if (v < 0) v = 0;
			for (y = 0; y < v; y++)
				block[63-y][x] = 0xffff;
			for ( ; y < 64; y++) {
              block[63-y][x] = bg;
              if (bg == 0 && y % 10 == 0)
                // draw 10dB/ scale on background. 10dB/10pixel
                block[63-y][x] = RGB565(15,15,15);
            }
			//i += stride;
		}
		ili9341_draw_bitmap(sx, 72, 32, 64, (uint16_t*)block);
	}
}

const struct { uint8_t r,g,b; } colormap[] = {
		{ 0, 0, 0 },
		{ 0, 0, 255 },
		{ 0, 255, 0 },
		{ 255, 0, 0 },
		{ 255, 255, 255 }
};

uint16_t
pick_color(int mag) /* mag: 0 - 63 */
{
	int idx = (mag >> 4) & 0x3;
	int prop = mag & 0x0f;
	int nprop = 0x10 - prop;
	int r = colormap[idx].r * nprop + colormap[idx+1].r * prop;
	int g = colormap[idx].g * nprop + colormap[idx+1].g * prop;
	int b = colormap[idx].b * nprop + colormap[idx+1].b * prop;
	return RGB565(r>>4, g>>4, b>>4);
}

void
waterfall_init(void)
{
#if 0
  // Vertical Scroll Definition
  uint16_t tfa = 152;
  uint16_t vsa = 240 - tfa;
  uint16_t bfa = 80;
  uint8_t vsd[6] = { tfa>>8, tfa, vsa>>8, vsa, bfa>>8, bfa };
  send_command(0x33, 6, vsd);
#endif
}

#define YPOS 152
#define HEIGHT 88
#define BLOCK_WIDTH 46
/* 46 * 88 = 4048 pixels < sizeof spi_buffer (4096) */
/* 320 / 46 = 6.96  -> draw block 7 times */

// FS=+-44
//   22-16=6 : +-352
//   22-3-16=3 : +-2816

static inline int
v2ypos(q31_t v)
{
  v >>= 24;
  v += HEIGHT/2;
  if (v < 0) v = 0;
  if (v >= HEIGHT) v = HEIGHT-1;
  return v;
}

static inline int
inrange(int v0, int v1, int y)
{
  if (v0 <= v1) {
    return v0 <= y && y <= v1;
  } else {
    return v1 <= y && y < v0;
  }
}

#define FG_SCALE RGB565(15,15,15)

void
draw_waveform(void)
{
	q31_t *buf = spdispinfo.buffer;
	uint16_t fg = uistat.mode == WFDISP ? FG_ACTIVE : FG_SCALE;
	uint16_t bg = BG_NORMAL;
    uint16_t c;
	int x, y;
    int xx;
    int w;
    int i;

    if (uistat.wfdispmode == WATERFALL)
      return;

    if (uistat.wfdispmode == WAVEFORM_MAG2)
      mag_shift = 6;
    else if (uistat.wfdispmode == WAVEFORM_MAG)
      mag_shift = 3;
    else
      mag_shift = 0;

    int i0 = v2ypos(buf[(512-160)*2-2]);
    int q0 = v2ypos(buf[(512-160)*2-1]);
    int i1 = v2ypos(buf[(512-160)*2+0]);
    int q1 = v2ypos(buf[(512-160)*2+1]);

    xx = 0;
	for (x = 0; x < 320; ) {
      w = BLOCK_WIDTH;
      if (w > (320 - x))
        w = 320 - x; // adjust last section width

      for (i = 0; i < w; i++) {
        int i2 = v2ypos(buf[(512-160)*2+2 + x*2]);
        int q2 = v2ypos(buf[(512-160)*2+3 + x*2]);

        int imin = i1;
        int qmin = q1;
        int imax = i1;
        int qmax = q1;
        if (imin > (i0+i1)/2) imin = (i0+i1)/2;
        if (imax < (i0+i1)/2) imax = (i0+i1)/2;
        if (imin > (i1+i2)/2) imin = (i1+i2)/2;
        if (imax < (i1+i2)/2) imax = (i1+i2)/2;
        if (qmin > (q0+q1)/2) qmin = (q0+q1)/2;
        if (qmax < (q0+q1)/2) qmax = (q0+q1)/2;
        if (qmin > (q1+q2)/2) qmin = (q1+q2)/2;
        if (qmax < (q1+q2)/2) qmax = (q1+q2)/2;
        
        for (y = 0; y < HEIGHT; y++) {
          c = bg;
          // draw origin line
          if (y == HEIGHT/2)
            c = fg;
          // draw 1ms tick
          if (x % 48 == 0)
            c = fg;

          if (y == i1 || (imin < y && y <= imax))
            c |= RGB565(255, 255, 0);
          if (y == q1 || (qmin < y && y <= qmax))
            c |= RGB565(255, 0, 255);
          
          spi_buffer[y * w + i] = c;
        }
        i0 = i1;
        q0 = q1;
        i1 = i2;
        q1 = q2;
        x++;
      }
      
      ili9341_draw_bitmap(xx, 152, w, HEIGHT, spi_buffer);
      xx += i;
	}
}

static int16_t vsa = 152;

void
draw_waterfall(void)
{
	int x;
	q31_t *buf = spdispinfo.buffer;
	uint16_t mode = uistat.spdispmode;
    const spectrumdisplay_param_t *param = GET_SPDISP_PARAM(mode);
    if (uistat.fs == 192)
      param = &spdispparam_tbl_192khz[mode];
	uint16_t *block = spi_buffer;
	int i = param->offset;
	int stride = param->stride;
	uint16_t bg = uistat.mode == WFDISP ? BG_ACTIVE : BG_NORMAL;
    uint16_t c;

    if (uistat.wfdispmode != WATERFALL)
      return;
    
	for (x = 0; x < 320; x++) {
        int64_t acc = 0;
        int i0 = i;
        for (; i < i0 + stride; i++) {
          q31_t ii = buf[(i&1023)*2];
          q31_t qq = buf[(i&1023)*2+1];
          acc += (int64_t)ii*ii + (int64_t)qq*qq;
        }
		//q31_t ii = buf[(i&1023)*2];
		//q31_t qq = buf[(i&1023)*2+1];
		//q31_t mag = ((int64_t)ii*ii + (int64_t)qq*qq)>>(33-gainshift);
		//int v = log2_q31(mag) >> 6;
        int v = (log2_i64(acc) - (34<<8)) >> 6;
		if (v < 0) v = 0;
		if (v > 63) v = 63;
        c = pick_color(v);
        if (c == 0)
          c = bg;
		*block++ = c;
		//i += stride;
	}

	vsa++;
	if (vsa >= 240)
		vsa = 152;
#if 0
	// Vertical Scroll Address
	uint8_t vscrsadd[2] = { vsa>>8, vsa };
	send_command(0x37, 2, vscrsadd);
#endif
	ili9341_draw_bitmap(0, vsa, 320, 1, spi_buffer);
}

static void
itoap(int value, char *buf, int dig, int pad)
{
  char neg = 0;
  if (dig == 0) {
    itoa(value, buf, 10);
    return;
  }
  if (value < 0) {
    neg = '-';
    value = -value;
  }
  
  buf[dig--] = '\0';
  do {
    buf[dig--] = (value % 10) + '0';
    value /= 10;
  } while (value > 0 && dig >= 0);
  if (neg && dig >= 0) {
    buf[dig--] = neg;    
  }
  while (dig >= 0) {
    buf[dig--] = pad;
  }
}

void
draw_tick_abs(void)
{
	char str[10];
	uint16_t mode = uistat.spdispmode;
    const spectrumdisplay_param_t *param = GET_SPDISP_PARAM(mode);
	uint16_t bg = BG_NORMAL;
	uint16_t fg = uistat.mode == SPDISP ? FG_ACTIVE : FG_NORMAL;
	int offset = param->origin;
	//int step = param->tickstep;
	int unit = param->tickunit;
    int fs = uistat.fs;
    float step = unit * 1024.0 / (fs * param->stride);
    float freq = center_frequency;
    float x;
    
    ili9341_fill(0, 136, 320, 16, bg);

    freq -= unit * 1000 * offset / step;
    freq /= 1000.0; // into kHz
    float m = freq / unit;
    m -= floor(m);
    m *= unit;
    freq -= m;
    x = -m * step / unit;
	while (x < 320) {
      itoap((int)freq % 1000, str, 3, '0');
      if (x >= 0)
        ili9341_fill((int)x, 136, 2, 5, fg);
      ili9341_drawstring_5x7(str, (int)x - 7, 142, fg, bg);
      x += step;
      freq += unit;
	}

    //ili9341_fill(offset, 136, 2, 6, RGB565(128,255,128));
}

void
draw_tick(void)
{
	char str[10];
	uint16_t mode = uistat.spdispmode;
    if (mode == 0) {
      draw_tick_abs();
      return;
    }
    const spectrumdisplay_param_t *param = GET_SPDISP_PARAM(mode);
	int x = param->origin;
	int base = param->tickbase;
	int xx;
	uint16_t bg = BG_NORMAL;
	uint16_t fg = uistat.mode == SPDISP ? FG_ACTIVE : FG_NORMAL;

	ili9341_fill(0, 136, 320, 16, bg);
    itoa(base, str, 10);
    strcat(str, param->unitname);
	xx = x - strlen(str) * 5 / 2;
	if (xx < 0) xx = 0;
	ili9341_drawstring_5x7(str, xx, 142, fg, bg);
	ili9341_fill(x, 136, 2, 5, fg);

	base += param->tickunit;
	x += param->tickstep;
	while (x < 320) {
      itoa(base, str, 10);
		ili9341_fill(x, 136, 2, 5, 0xffff);
        int xx = x - strlen(str) * 5 / 2;
		ili9341_drawstring_5x7(str, xx, 142, fg, bg);
		base += param->tickunit;
		x += param->tickstep;
	}
	x = param->origin;
	base = param->tickbase;
	base -= param->tickunit;
	x -= param->tickstep;
	while (x >= 0) {
      itoa(base, str, 10);
		ili9341_fill(x, 136, 2, 5, 0xffff);
        int xx = x - strlen(str) * 5 / 2;
		ili9341_drawstring_5x7(str, xx, 142, fg, bg);
		base -= param->tickunit;
		x -= param->tickstep;
	}
}

void
draw_freq(void)
{
	char str[10];
	uint16_t bg = BG_NORMAL;
	int i;
	const uint16_t xsim[] = { 0, 16, 0, 0, 16, 0, 0, 0 };
	uint16_t x = 0;
    itoap(uistat.freq, str, 8, ' ');
	for (i = 0; i < 8; i++) {
		int8_t c = str[i] - '0';
		uint16_t fg = 0xffff;
        int focused = FALSE;
		if (uistat.mode == FREQ && uistat.digit == 7-i) {
            fg = FG_ACTIVE;
            focused = TRUE;
        }
		if (c >= 0 && c <= 9)
			ili9341_drawfont(c, &NF32x48, x, 0, fg, bg);
		else if (focused)
            ili9341_drawfont(0, &NF32x48, x, 0, fg, bg);
        else
			ili9341_fill(x, 0, 32, 48, bg);
		x += 32;

		// fill gaps
		if (xsim[i] > 0) {
			ili9341_fill(x, 0, xsim[i], 48, bg);
			x += xsim[i];
		}
	}
	// draw Hz symbol
	ili9341_drawfont(10, &NF32x48, x, 0, 0xffff, bg);
}

void
draw_channel_freq(void)
{
	char str[10];
	uint16_t bg = BG_NORMAL;
	uint16_t fg = uistat.mode == CHANNEL ? FG_ACTIVE : FG_NORMAL;
	int i;
	const uint16_t xsim[] = { 0, 16, 0, 0, 0 };
	uint16_t x = 0;

    ili9341_fill(x, 0, 20*2, 24, bg);
    itoap(uistat.channel, str, 2, ' ');
	ili9341_drawfont(uistat.channel / 10, &NF20x24, x, 24, fg, bg);
    x += 20;
	ili9341_drawfont(uistat.channel % 10, &NF20x24, x, 24, fg, bg);
    x += 20;

    bg = BG_NORMAL;
    ili9341_fill(x, 0, 52, 48, bg);
    x += 24+12+16;

    itoap(uistat.freq / 1000, str, 5, ' ');
	for (i = 0; i < 5; i++) {
		int8_t c = str[i] - '0';
		if (c >= 0 && c <= 9)
			ili9341_drawfont(c, &NF32x48, x, 0, FG_NORMAL, bg);
        else
			ili9341_fill(x, 0, 32, 48, bg);
		x += 32;

		// fill gaps
		if (xsim[i] > 0) {
			ili9341_fill(x, 0, xsim[i], 48, bg);
			x += xsim[i];
		}
	}
	// draw kHz symbol
	ili9341_drawfont(11, &NF32x48, x, 0, FG_NORMAL, bg);
    x += 20;
	ili9341_drawfont(10, &NF32x48, x, 0, FG_NORMAL, bg);
}

#define FG_VOLUME 0xfffe
#define FG_MOD RGB565(255,0,0)
#define FG_AGC RGB565(255,255,0)


void
draw_db(int db, int x, int y, int16_t fg, int16_t bg)
{
  char str[10];
  ili9341_drawfont(10, &NF20x24, x+80-5, y, fg, bg); // period
  int d = db >> 8;
  if (d < 0 && (db & 0xff))
    d++;
  itoap(d, str, 4, ' ');
  ili9341_drawfont_string(str, &NF20x24, x, y, fg, bg);
  if (db < 0) db = 0x100 - (db & 0xff);
  itoap(((db & 0xff) * 10) >> 8, str, 1, ' ');
  ili9341_drawfont_string(str, &NF20x24, x+100-10, y, fg, bg);
  x += 120-10;
  ili9341_drawfont(13, &NF20x24, x, y, fg, bg); // dB
  x += 20;
  ili9341_fill(x, y, 6, 32, bg);
}

void
draw_dbm(int db, int x, int y, int16_t fg, int16_t bg)
{
  char str[10];
  ili9341_drawfont(10, &NF20x24, x+80-6, y, fg, bg); // period
  ili9341_drawfont(22, &NF20x24, x+96, y, fg, bg); // d
  ili9341_drawfont(23, &NF20x24, x+116, y, fg, bg); // Bm
  int d = db >> 8;
  if (d < 0 && (db & 0xff))
    d++;
  itoap(d, str, 4, ' ');
  ili9341_drawfont_string(str, &NF20x24, x, y, fg, bg);
  if (db < 0) db = 0x100 - (db & 0xff);
  itoap(((db & 0xff) * 10) >> 8, str, 1, ' ');
  ili9341_drawfont_string(str, &NF20x24, x+100-14, y, fg, bg);
}

void
draw_info(void)
{
	char str[10];
	int x = 0;
	int y = 48;
	uint16_t bg = BG_NORMAL;
    uint16_t fg = uistat.mode == VOLUME ? FG_ACTIVE : FG_VOLUME;
	ili9341_drawfont(14, &NF20x24, x, y, fg, bg);
	x += 20;
	if (uistat.volume != -7)
      itoap(uistat.volume, str, 2, ' ');
	else
		// -infinity
		strcpy(str, "-\003");
	ili9341_drawfont_string(str, &NF20x24, x, y, fg, bg);
	x += 40;
	ili9341_drawfont(13, &NF20x24, x, y, fg, bg); // dB
	x += 20;

	fg = uistat.mode == MOD ? FG_ACTIVE : FG_MOD;
	ili9341_drawfont(uistat.modulation, &ICON48x20, x+2, y+2, fg, bg);
	x += 48+4;

	fg = uistat.mode == AGC ? FG_ACTIVE : FG_AGC;
	ili9341_drawfont(uistat.agcmode + ICON_AGC_OFF, &ICON48x20, x+2, y+2, fg, bg);
	x += 48+4;

	fg = uistat.mode == AGC ? FG_ACTIVE : FG_NORMAL;
    if (uistat.mode == RFGAIN) {
      fg = FG_ACTIVE;
      draw_db(uistat.rfgain << 7, x, y, fg, bg);

      // draw antenna icon by mode color of analog/digital
      fg = 0x07ff;
      if (uistat.rfgain < 0 || uistat.rfgain >= 96)
        fg = 0x070f;
      ili9341_drawfont(15, &NF20x24, x, y, fg, bg); 
    }
}

void
draw_aux_info(void)
{
	char str[10];
	int x = 0;
	int y = 48;
	uint16_t bg = BG_NORMAL;
    uint16_t fg = FG_NORMAL;

    if (uistat.mode == AGC_MAXGAIN) {
      fg = BG_NORMAL; bg = FG_NORMAL;
    }
      
    ili9341_drawstring_5x7("AGCMAX", x, y, fg, bg);
    x += 30;
    itoap(config.agc.maximum_gain, str, 6, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);

    y += 8;
    x = 0;
    fg = FG_NORMAL; bg = BG_NORMAL;
    if (uistat.mode == CWTONE) {
      fg = BG_NORMAL; bg = FG_NORMAL;
    }
      
    ili9341_drawstring_5x7("CWTONE", x, y, fg, bg);
    x += 30;
    itoap(uistat.cw_tone_freq, str, 4, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);
    x += 20;
    ili9341_drawstring_5x7("Hz", x, y, fg, bg);

    y += 8;
    x = 0;
    fg = FG_NORMAL; bg = BG_NORMAL;
    if (uistat.mode == IQBAL) {
      fg = BG_NORMAL; bg = FG_NORMAL;
    }

    ili9341_drawstring_5x7("IQBAL ", x, y, fg, bg);
    x += 30;
    itoap(uistat.iqbal, str, 6, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);
    x += 20;

    // show statistics
    fg = FG_NORMAL; bg = BG_NORMAL;
	y = 48;
	x = 65;
    ili9341_drawstring_5x7("BATT", x, y, fg, bg);
    x += 20;
    itoap(stat.battery, str, 5, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);
    y += 8;
	x = 65;
    ili9341_drawstring_5x7("TEMP", x, y, fg, bg);
    x += 20;
    itoap(stat.temperature, str, 5, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);
    y += 8;
	x = 65;
    ili9341_drawstring_5x7("VREF", x, y, fg, bg);
    x += 20;
    itoap(stat.vref, str, 5, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);

    fg = FG_NORMAL; bg = BG_NORMAL;
	y = 48;
	x = 115;
    ili9341_drawstring_5x7("RMS", x, y, fg, bg);
    x += 15;
    itoap(stat.rms[0], str, 5, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);
    y += 8;
	x = 115;
    ili9341_drawstring_5x7("FPS", x, y, fg, bg);
    x += 15;
    itoap(stat.fps, str, 5, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);
    y += 8;
	x = 115;
    ili9341_drawstring_5x7("OVF", x, y, fg, bg);
    x += 15;
    itoap(stat.overflow, str, 5, ' ');
    ili9341_drawstring_5x7(str, x, y, fg, bg);
}

void clear_aux_info(void)
{
  ili9341_fill(0, 48, 184, 24, 0x0000);
}

void
draw_power(void)
{
  int x = 184;
  int y = 48;
  // draw power value not only ui mode is at RFGAIN
  if (uistat.mode != RFGAIN) {
    draw_dbm(measured_power_dbm, x, y, 0xffff, 0x0000);
  }
}

void
clear_background(void)
{
	int i = 0;
	for (i = 0; i < 24; i++) {
		ili9341_fill(0, i*10, 320, 10, 0x0000);
	}
}

// called periodically
void
disp_process(void)
{
  if (spdispinfo.update_flag & FLAG_SPDISP) {
    draw_waveform();
    draw_spectrogram();
    draw_waterfall();
    spdispinfo.update_flag &= ~FLAG_SPDISP;
  }

  if (spdispinfo.update_flag & FLAG_AUX_INFO) {
    clear_aux_info();
    spdispinfo.update_flag &= ~FLAG_AUX_INFO;
  }
  
  if (spdispinfo.update_flag & FLAG_UI) {
    draw_tick();
    if (uistat.mode == CHANNEL)
      draw_channel_freq();
    else
      draw_freq();
    if (uistat.mode != AGC_MAXGAIN && uistat.mode != CWTONE && uistat.mode != IQBAL)
      draw_info();
    else
      draw_aux_info();
    spdispinfo.update_flag &= ~FLAG_UI;
  }

  if (spdispinfo.update_flag & FLAG_POWER) {
    draw_power();

    //if (uistat.mode == AGC_MAXGAIN || uistat.mode == CWTONE || uistat.mode == IQBAL)
    //draw_aux_info();

    spdispinfo.update_flag &= ~FLAG_POWER;
  }
}

void
disp_update(void)
{
  spdispinfo.update_flag |= FLAG_UI;
}
   
void
disp_update_power(void)
{
  spdispinfo.update_flag |= FLAG_POWER;
}

void
disp_clear_aux(void)
{
  spdispinfo.update_flag |= FLAG_AUX_INFO;
}

void
disp_init(void)
{
  arm_cfft_radix4_init_q31(&cfft_inst, 1024, FALSE, TRUE);
  waterfall_init();
  clear_background();
  spdispinfo.update_flag = FLAG_UI;
}
